#include <stdio.h>
#include <stdlib.h>
#include <pthread.h>
#include <string.h>
#include <unistd.h>
#include <sys/select.h>
#include "common.h"
#include "canbus.h"
#include "osthread.h"
#include "uart.h"
#include "ethserver.h"
#include "communicate.h"
#include "RobotConfig.h"
#include "robot.h"
#include "dataqueue.h"
#include "motorreport.h"
#include "datamanger.h"
#include "navigation.h"
#include "odom.h"



extern RobotServo_t RobotServo[];  //由于是统一编号
extern char buff[BUFF_SIZE];   //定义缓冲区
extern int handlefd[MAXFD];


void *cansend_thread();
void *uartsend_thread();
void *ethsend_thread();
void *udpsend_thread();
void *ethrecv_thread();
void *canrecv_thread();
void *uartrecv_thread();
void *robotinfoupdate_thread();
void *startkeya_thread();
void *event_handler_thread();


pthread_t *startkeyathreadid;           //keya驱动器循环控制指令
pthread_t *robotinfoupdatethreadid;     //机器人信息上传线程
pthread_t *event_handlerthreadid;



void thread_create(void)
{   
    printf("Creating threads!\n");
    //dumpthreadid = (pthread_t *) malloc (sizeof (pthread_t));   //上文定义的是一个指针，需要给指针指向一个明确的内存地址
    startkeyathreadid = (pthread_t *) malloc (sizeof (pthread_t));
    robotinfoupdatethreadid = (pthread_t *) malloc (sizeof (pthread_t));
    event_handlerthreadid = (pthread_t *) malloc (sizeof (pthread_t));

    queue_init();       //初始化所有工作队列
    pthread_create(cansendqueue->queuethreadid, NULL, cansend_thread, NULL);        //创建can发送线程
    pthread_create(uartsendqueue->queuethreadid, NULL, uartsend_thread, NULL);      //创建uart发送线程
    pthread_create(ethsendqueue->queuethreadid, NULL, ethsend_thread, NULL);        //创建tcp发送线程
    pthread_create(udpsendqueue->queuethreadid, NULL, udpsend_thread, NULL);        //创建udp发送线程
    
    pthread_create(canrecvqueue->queuethreadid, NULL, canrecv_thread, NULL);        //创建tcp指令解析分发线程
    pthread_create(uartrecvqueue->queuethreadid, NULL, uartrecv_thread, NULL);      //创建uart发送线程 
    pthread_create(ethrecvqueue->queuethreadid, NULL, ethrecv_thread, NULL);        //创建tcp指令解析分发线程
    
    pthread_create(robotinfoupdatethreadid, NULL, robotinfoupdate_thread, NULL);    //机器人信息读取
    pthread_create(startkeyathreadid, NULL, startkeya_thread, NULL);                //科亚持续发送速度指令
    pthread_create(event_handlerthreadid, NULL, event_handler_thread,NULL);   //事件处理
    //前进固定距离，停下后等待时间。转动角度
    //udp发送


}


/**
  * can发送线程，等待其他线程给出条件变量信号
  * 
  * 调用can底层发送函数
  * */ 
void *cansend_thread()
{ 
    printf("enter can SEND thread!\n");
    while (1){
        pthread_mutex_lock(&(cansendqueue->queue_lock));

        while (cansendqueue->cur_queue_size == 0){ //如果没有任务就等待
            //printf ("***SEND PTHREAD 0x%x is waiting\n", pthread_self ());
            pthread_cond_wait(&(cansendqueue->queue_ready), &(cansendqueue->queue_lock)); //等待！
        }
        // printf("CAN data need to be send!\n");
         
        //开始处理任务
        cansendqueue->cur_queue_size--;
        Cthread_cantask *task = cansendqueue->queue_head;
        cansendqueue->queue_head = task->next;

        pthread_mutex_unlock (&(cansendqueue->queue_lock));

        //调用can发送函数
        if(handlefd[CANFD] < 0) //如果can初始化失败则跳出
        {
            printf("can init failed, can send thread exit!!!\n");
            pthread_exit (NULL); //退出线程
        }
            
        canbus_send(handlefd[CANFD], task->id, task->data, task->len);

        free (task);
        task = NULL;
    }
    /*这一句应该是不可达的*/
    pthread_exit (NULL);

}

/**
  * uart发送线程，等待其他线程给出条件变量信号
  * 
  * 调用uart底层发送函数
  * */ 
void *uartsend_thread()
{
    printf("enter UART thread!\n");
    while (1){
        pthread_mutex_lock(&(uartsendqueue->queue_lock));

        while (uartsendqueue->cur_queue_size == 0){ //如果没有任务就等待
            pthread_cond_wait(&(uartsendqueue->queue_ready), &(uartsendqueue->queue_lock)); //等待！
        }
        //printf("UART:having data need to be send!\n");
         
        //开始处理任务
        uartsendqueue->cur_queue_size--;
        Cthread_uarttask *task = uartsendqueue->queue_head;
        uartsendqueue->queue_head = task->next;


        pthread_mutex_unlock (&(uartsendqueue->queue_lock));

        //调用uart发送函数
        uart_wirte(task->data, UART_CMD_LENGTH);

        free (task);
        task = NULL;
    }
    /*这一句应该是不可达的*/
    pthread_exit (NULL);

}

/**
  * tcp发送线程，等待其他线程给出条件变量信号
  * 
  * 调用tcp底层发送函数
  * */ 
void *ethsend_thread()
{
    printf("enter ETH send thread!\n");

    while (1){
        pthread_mutex_lock(&(ethsendqueue->queue_lock));

        while (ethsendqueue->cur_queue_size == 0){ //如果没有任务就等待
            //printf ("***SEND PTHREAD 0x%x is waiting\n", pthread_self ());
            pthread_cond_wait(&(ethsendqueue->queue_ready), &(ethsendqueue->queue_lock)); //等待！
        }
        //printf("ETH data need to be send!\n");
         
        //开始处理任务
        ethsendqueue->cur_queue_size--;
        Cthread_ethtask *task = ethsendqueue->queue_head;
        ethsendqueue->queue_head = task->next;


        pthread_mutex_unlock (&(ethsendqueue->queue_lock));

        //调用eth发送函数
        eth_send_package(task->data);

        free (task);
        task = NULL;
        usleep(1000);
    }
    /*这一句应该是不可达的*/
    pthread_exit (NULL);

}

void *udpsend_thread()
{
    printf("enter udp_send thread!\n");
    while (1){
        pthread_mutex_lock(&(udpsendqueue->queue_lock));

        while (udpsendqueue->cur_queue_size == 0){ //如果没有任务就等待
            //printf ("***SEND PTHREAD 0x%x is waiting\n", pthread_self ());
            pthread_cond_wait(&(udpsendqueue->queue_ready), &(udpsendqueue->queue_lock)); //等待！
        }
        
        //开始处理任务
        udpsendqueue->cur_queue_size--;
        Cthread_udptask *task = udpsendqueue->queue_head;
        udpsendqueue->queue_head = task->next;

        pthread_mutex_unlock(&(udpsendqueue->queue_lock));
        
        //调用底层发送函数
        udp_send_package(task->destip, task->destport, task->data, task->len);
        

        free(task);
        task = NULL;
    }
    /*这一句应该是不可达的*/
    pthread_exit (NULL);


}



/**
  * tcp接收命令处理线程，等待dump线程给出条件变量信号
  * 机器人任务实现主要线程
  * 接收来自上位机的指令后，在此线程中解析，分发指令，实现具体的机器人动作
  * */ 
void *ethrecv_thread()
{
    printf("enter data  analysis thread!\n");
 

    while (1){
        pthread_mutex_lock(&(ethrecvqueue->queue_lock));

        while (ethrecvqueue->cur_queue_size == 0){ //如果没有任务就等待
            //printf ("***SEND PTHREAD 0x%x is waiting\n", pthread_self ());
            pthread_cond_wait(&(ethrecvqueue->queue_ready), &(ethrecvqueue->queue_lock)); //等待！
        }
        
        //开始处理任务
        ethrecvqueue->cur_queue_size--;
        Cthread_ethtask *task = ethrecvqueue->queue_head;
        ethrecvqueue->queue_head = task->next;

        pthread_mutex_unlock(&(ethrecvqueue->queue_lock));
        
        //调用任务分发函数
        servertaskdistribute((ServerCmdStruct *)(task->data));
        

        free(task);
        task = NULL;
    }
    /*这一句应该是不可达的*/
    pthread_exit (NULL);

}



/**
  * can接收命令处理线程，等待dump线程给出条件变量信号
  * 
  * 接收来自驱动器的数据，在此线程中解析，更性数据
  * */ 
void *canrecv_thread()
{
    printf("enter canrecv_thread thread!\n");
    while(1){

        pthread_mutex_lock(&(canrecvqueue->queue_lock));

        while (canrecvqueue->cur_queue_size == 0){ //如果没有任务就等待
            //printf ("***SEND PTHREAD 0x%x is waiting\n", pthread_self ());
            pthread_cond_wait(&(canrecvqueue->queue_ready), &(canrecvqueue->queue_lock)); //等待！
        }

        canrecvqueue->cur_queue_size--;
        Cthread_cantask *task = canrecvqueue->queue_head;
        canrecvqueue->queue_head = task->next;

        pthread_mutex_unlock(&(canrecvqueue->queue_lock));  //释放锁，再robot_position_updata（此函数中也有锁）

        MotorDriveDataReported(task);           //使用信号量，等待驱动器的数据回复

        free (task);
        task = NULL;
    }

}


void *uartrecv_thread()
{
    printf("enter uartrecv_thread thread!\n");
    while (1){
        pthread_mutex_lock(&(uartrecvqueue->queue_lock));

        while (uartrecvqueue->cur_queue_size == 0){ //如果没有任务就等待
            pthread_cond_wait(&(uartrecvqueue->queue_ready), &(uartrecvqueue->queue_lock)); //等待！
        }
         
        //开始处理任务
        uartrecvqueue->cur_queue_size--;
        Cthread_uarttask *task = uartrecvqueue->queue_head;
        uartrecvqueue->queue_head = task->next;

        pthread_mutex_unlock (&(uartrecvqueue->queue_lock));

        //调用uart接受函数
        //printf("uart update: %02x %02x %02x %02x\n",task->data[0], task->data[1], task->data[2], task->data[3] );
        uart_updata((UartRecvCmd_t *)(task->data));


        free (task);
        task = NULL;
    }
    /*这一句应该是不可达的*/
    pthread_exit (NULL);


    
}


/**
  * 机器人信息更新
  * 1、电机电流、速度、位置
  * 2、电池电压
  * 3、传感器数据
  * */
void *robotinfoupdate_thread()
{
    uint8_t i;
    sleep(1);
    ROBOT_POSITION cur_position;
    while(1){
        if((i%1) == 0){     // 100mss
            MotorDriveMonitor();        //里程计获取
        }
        if((i%4) == 0){     //400ms
            MotorDriveMonitor();        //里程计获取
        }

        if((i%12) == 0){    //1200ms
            i = 0;
            //上报位置信息
        }
        
        i++;
        usleep(100000); //100ms
    }
    /*这一句应该是不可达的*/
    pthread_exit (NULL);
 
}



/**
  * KEYA驱动器需要连续的给定速度指令
  * 定义线程单独处理
  *  
  * */
void *startkeya_thread()
{
    uint8_t i;

    RobotServoAllInit();
    while(1){
        if(robotinfo_read(LINE_WARNING) == 0){
            ServoUseKEYARun((void *)(&RobotServo[0]));
            usleep(2000); //2ms
            ServoUseKEYARun((void *)(&RobotServo[1]));  
        }      
        usleep(800000); //800ms
    }

}


void *event_handler_thread()
{
    EVENT_INDEX event_index;

    while(1){
        event_index = get_current_event();      //阻塞
        switch(event_index){
            case EVENT_GO_DISTANCE:         //定点调整
                printf("EVENT_GO_DISTANCE \n");
                go_distance_handler();
            break;

            case EVENT_TURN_AROUND:         //转弯
                printf("EVENT_TURN_AROUND\n");
                turn_around_handler();
            break;

            default:
            break;

        }

    }
    /*这一句应该是不可达的*/
    pthread_exit (NULL);
}


void dump_thread()
{
    printf("enter  DUMP thread!\n");

    int i,j,len;
    int max_fd;    //最大的文件描述符
    char buff[128]; //接收缓冲区
    fd_set read_fds; //select相关
	int ret;

    static int eth_status = 0;             //网络链接状态
    
    //int sock_client; //建立链接的句柄，由accept分配  （套接字句柄ethfd 为保持监听转台，不能用于读写操作） 

    sleep(3);   //等待通讯接口初始化完成，否则获取不到文件描述符

    while(1){
        FD_ZERO(&read_fds);
        for(i=0; i<MAXFD ; i++){

            if(handlefd[i] < 0){    //接口初始化失败，跳过
                continue;
            }

            if(i == CLIENTFD){
                if(eth_status == 0)//判断是否连接，只有链接之后才可以设置eth接收
                    continue;
            }

            FD_SET(handlefd[i],&read_fds);

            if(handlefd[i] > max_fd)    //找到最大的文件描述符树
                max_fd = handlefd[i];
        }

        ret = select(max_fd+1,&read_fds,NULL,NULL,NULL); //等待有数据，触发select
        
        if(ret == 0){ //超时 
            printf("select time out!\n");
        }
        else if(ret == -1){  //失败
            printf("fail to select!\n");
        }
        else{  //成功
            for(i = 0; i < MAXFD ; i++){

                if(handlefd[i] < 0){    //通信接口初始化失败，跳过
                    continue;
                }

                if(FD_ISSET(handlefd[i],&read_fds)){
                    switch(i){
                        case CANFD://接收到can数据
                            ret = canbus_recv(handlefd[CANFD]);
                            if( ret < 0){
                                printf("CAN receive error!!!\n");
                            }
                        break;

                        case ETHFD: //处理链接
                            handlefd[CLIENTFD] = accept(handlefd[ETHFD],NULL, NULL);
                            if (handlefd[CLIENTFD] < 0) {
                                perror("accept error");
                            }else
                            {
                                printf("Client connected!\n");
                                eth_status = 1;     //连接标志置位
                            }    
                        break;

                        case CLIENTFD:  //处理接收的数据
                            if((len = recv(handlefd[CLIENTFD],buff,128,0)) >0)
                            {
                                recv_eth_data(buff, len);    //完整协议解析
                            }
                            else if(len <= 0)
                            {
                                close(handlefd[CLIENTFD]);
                                handlefd[CLIENTFD] = -1;
                                printf("Client disconnect!!\n");       
                            }
                        break;

                        case UARTFD:
                            if((len = uart_read(buff)) > 0);
                            {
                                recv_uart_data(buff,len);//添加到接收队列
                            }                           
                        break;
                        case UDPFD:
                            if((len = udp_recv(buff)) > 0)
                            {
                                recv_udp_data(buff, len);
                            }
                        break;
                        
                        default:
                            ("Having no fd  handle function!\n");
                        break;

                    }
                    //处理完，清除缓冲区的数据
                    len = 0;
                    memset(buff,0,sizeof(buff)); 
                }

            }
                
        }

    }	
}






